//#include "usart_plugin.h"

#include <ros/ros.h> 
#include <serial/serial.h>  //ROS已经内置了的串口包 
#include <string>
#include <std_msgs/String.h> 
#include <time.h>

#include <std_msgs/Empty.h> 
#include "serialport/ALL_UAVs_LLA.h"
#include "geographic_msgs/GeoPoint.h"

using namespace std;

serial::Serial ser; //声明串口对象 

uint8_t split(double data, int position);    //将64位的double变成uint32_t再变成uint8_t
double merge(uint8_t first, uint8_t second, uint8_t third, uint8_t fourth); //将4个uint8_t转成uint32_t,再变成double
uint8_t data_crc(uint8_t *p_data, int len_data);   //接收校验
const int Blen_child = 17;    //子节点的数据长度
const int Blen_host  = 54;    //主节点的数据长度
const int Blen_stop  = 9;     //子节点信息发送许可证---信息
const int Blen_life  = 6;     //子节点信息发送许可证---信息
uint8_t recv_buff[100];
uint8_t send_buff[17];
uint8_t life_buff[6];
// double longitude = 0, latitude = 0, altitude = 0;
// int fail_num = 0,uav_recv_all = 0;
int len_send, lend_recv;
int uav_ID;
struct uav_MSG
{
    double longitude;
    double latitude;
    double altitude;
};
    struct SINFO
    {
        int send_num;
        int send_r_num;
        float send_r_rate;
        int recv_num;
        int recv_m1_r_num;
        int recv_m2_r_num;
        float recv_r_rate;
        int fail_num;
    };
 SINFO s_info = {};
uav_MSG uav0={},uav1={},uav2={},uav3={};

bool timeout = 0, loop_state, send_state =1,recv_state = 1;
clock_t start_time=0,end_time=0;
int time_dif=0;

void recv_usart(void);


int main (int argc, char** argv) 
{ 
    //初始化节点 
    ros::init(argc, argv, "usart_plugin"); 
    //声明节点句柄 
    ros::NodeHandle nh;
    ros::Rate loop_rate(20);     //设置循环频率，括号内参数即为频率 
    ros::Publisher all_position_pub = nh.advertise<serialport::ALL_UAVs_LLA>("/all_uavs_position/global", 10);

    serialport::ALL_UAVs_LLA all_uavs_position;
    geographic_msgs::GeoPoint geo_msg[3];
    std::vector< geographic_msgs::GeoPoint> arrp;
    try 
    { 
        //设置串口属性，并打开串口 
        ser.setPort("/dev/ttyUSB3"); 
        ser.setBaudrate(115200); 
        serial::Timeout to = serial::Timeout::simpleTimeout(1000); 
        ser.setTimeout(to); 
        ser.open(); 
    } 
    catch (serial::IOException& e) 

    { 
        ROS_ERROR_STREAM("Unable to open port "); 
        return -1; 
    } 
    //检测串口是否已经打开，并给出提示信息 
    if(ser.isOpen()) 
    { 
        ROS_INFO_STREAM("Serial Port initialized"); 
    } 
    else 
    { 
        return -1; 
    }

    // 地面站信息模拟
    life_buff[0] = 0xa4;
    life_buff[1] = 0xc0;
    life_buff[2] = 0x0;
    life_buff[3] = 0x1;
    life_buff[4] = data_crc(life_buff,Blen_life-2);   //  校验
    life_buff[5] = 0x4a;

    while(ros::ok()) 
    { 
        cout <<  endl;
        if (send_state)
        {
            int gb;
            cout << "please input gb:";
            cin >> gb;
            if (gb)
            {
                for (int i=0;i<10;i++)
                {
                    len_send = ser.write(life_buff,Blen_life);
                }
            }
            // loop_rate.sleep();
            send_state = 0;
        }
        /**********************************************************************************************************
        //  更新自身信息****************************************************
        uav2.longitude = 122.2135413;
        uav2.latitude = 22.2165385;
        uav2.altitude = 2.12156348;
        //  时间溢出判断****************************************************
        // end_time = clock();
        // time_dif = end_time - start_time;
        // if (time_dif > 160)     //160个机器时间
        //     timeout = 1; 
        // else
        //     timeout = 0; 
        //  发送自身信息****************************************************
        // if (send_state == 1)//|| timeout)
        // {
            //  信息的构造
            send_buff[0] = 0xa4;    //  帧头
            send_buff[1] = 0xc2;    //  命令
            send_buff[2] = 0x01;    //  命令
            send_buff[3] = split(uav2.longitude,1);  send_buff[4] = split(uav2.longitude,2); send_buff[5] = split(uav2.longitude,3); send_buff[6] = split(uav2.longitude,4);
            send_buff[7] = split(uav2.latitude,1);      send_buff[8] = split(uav2.latitude,2);     send_buff[9] = split(uav2.latitude,3);     send_buff[10] = split(uav2.latitude,4);
            send_buff[11] = split(uav2.altitude,1);    send_buff[12] = split(uav2.altitude,2);   send_buff[13] = split(uav2.altitude,3);  send_buff[14] = split(uav2.altitude,4);
            send_buff[15] = data_crc(send_buff,Blen_child-2);   //  校验
            send_buff[16] = 0x4a;   //  帧尾
            //  信息发送
            len_send = ser.write(send_buff,Blen_child);
            //  效果记录
            s_info.send_num++;
            // ros::Duration(0.05).sleep(); //sleep for 50 ms
            if (len_send == Blen_child)
            {
                // cout << "send successful!" << endl;
                s_info.send_r_num++;
            }
            //  重置时间信息
            // timeout = 0;
            // start_time = clock();
        // }
       ****************************************************************************************************** */
        //  接收信息****************************************************
        // recv_state = 1;
        // while(recv_state && !timeout)
        // {

           
            //时间检查更新
            // end_time = clock();
            // time_dif = end_time - start_time;
            // if (time_dif > 100)
            //     {timeout = 1; send_state = 1;}
            // else
            //     timeout = 0; 
        // }
        recv_usart();
        //发布消息*************************************************************************
        geo_msg[0].latitude=uav0.latitude;
        geo_msg[1].latitude=uav1.latitude;
        geo_msg[2].latitude=uav2.latitude;
        geo_msg[3].latitude=uav3.latitude;
        geo_msg[0].longitude=uav0.longitude;
        geo_msg[1].longitude=uav1.longitude;
        geo_msg[2].longitude=uav2.longitude;
        geo_msg[3].longitude=uav3.longitude;
        geo_msg[0].altitude=uav0.altitude;
        geo_msg[1].altitude=uav1.altitude;
        geo_msg[2].altitude=uav2.altitude;
        geo_msg[3].altitude=uav3.altitude;
        arrp.push_back(geo_msg[0]);
        arrp.push_back(geo_msg[1]);
        arrp.push_back(geo_msg[2]);
        arrp.push_back(geo_msg[3]);
        all_uavs_position.all_uavs_lla =arrp;
        all_position_pub.publish(all_uavs_position);
        //显示信息*************************************************************************
        cout << "uav0.longitude:" << uav0.longitude << "  " << "uav0.latitude:" << uav0.latitude << "  " << "uav0.altitude:" << uav0.altitude << endl;
        cout << "uav1.longitude:" << uav1.longitude << "  " << "uav1.latitude:" << uav1.latitude << "  " << "uav1.altitude:" << uav1.altitude << endl;
        cout << "uav2.longitude:" << uav2.longitude << "  " << "uav2.latitude:" << uav2.latitude << "  " << "uav2.altitude:" << uav2.altitude << endl;
        cout << "uav3.longitude:" << uav3.longitude << "  " << "uav3.latitude:" << uav3.latitude << "  " << "uav3.altitude:" << uav3.altitude << endl;
        //数据处理
        uav0= {};uav1 = {};uav2 = {};uav3 = {};uav_ID=0;
        //处理ROS的信息，比如订阅消息,并调用回调函数 
        ros::spinOnce(); 
        loop_state = loop_rate.sleep();
        // cout << "loop_state:  " << loop_state << endl;
        // cout << "recv_fail_num:" << s_info.fail_num << endl;
        // cout << "recv_m1_r_num:" << s_info.recv_m1_r_num << endl;
        // cout << "recv_m2_r_num:" << s_info.recv_m2_r_num << endl;
        // cout << "send_num: " << s_info.send_num << "\t" << "send_r_num: " << s_info.send_r_num << endl;
        // cout << "recv_num: " << s_info.recv_num << endl;
        // cout <<  endl;
    } 
    return 0;
}


void recv_usart(void)
{
  lend_recv = ser.read(recv_buff,81); 
            if (lend_recv) 
            {
                s_info.recv_num++;
                cout << "lend_recv:" << lend_recv <<endl;
                for (int i =0;i<lend_recv;i++)
                {
                    printf("%#x",recv_buff[i]);
                }
                cout << endl;
            }
            // 信息的解析
            int i = 0;
            while (i < lend_recv)
            {
                if (recv_buff[i] == 0xa4 && recv_buff[i+Blen_host-1] == 0x4a)
                {
                    // 接收消息1
                    if (recv_buff[i+1] == 0xf1 && recv_buff[i+2] == 0x01 && (lend_recv-i) >= Blen_host)
                    {
                        cout << "11111111111111" << endl;
                        // 校验位检验
                        if (data_crc(&recv_buff[i],Blen_host-2) == recv_buff[i+Blen_host-2])
                        {
                            recv_state = 1;
                            s_info.recv_m1_r_num++;
                            uav0.longitude = merge(recv_buff[i+3],recv_buff[i+4],recv_buff[i+5],recv_buff[i+6]);
                            uav0.latitude =     merge(recv_buff[i+7],recv_buff[i+8],recv_buff[i+9],recv_buff[i+10]);
                            uav0.altitude =     merge(recv_buff[i+11],recv_buff[i+12],recv_buff[i+13],recv_buff[i+14]);
                            uav1.longitude = merge(recv_buff[i+15],recv_buff[i+16],recv_buff[i+17],recv_buff[i+18]);
                            uav1.latitude =     merge(recv_buff[i+19],recv_buff[i+20],recv_buff[i+21],recv_buff[i+22]);
                            uav1.altitude =     merge(recv_buff[i+23],recv_buff[i+24],recv_buff[i+25],recv_buff[i+26]);
                            uav2.longitude = merge(recv_buff[i+27],recv_buff[i+28],recv_buff[i+29],recv_buff[i+30]);
                            uav2.latitude =     merge(recv_buff[i+31],recv_buff[i+32],recv_buff[i+33],recv_buff[i+34]);
                            uav2.altitude =     merge(recv_buff[i+35],recv_buff[i+36],recv_buff[i+37],recv_buff[i+38]);
                            uav3.longitude = merge(recv_buff[i+39],recv_buff[i+40],recv_buff[i+41],recv_buff[i+42]);
                            uav3.latitude =     merge(recv_buff[i+43],recv_buff[i+44],recv_buff[i+45],recv_buff[i+46]);
                            uav3.altitude =     merge(recv_buff[i+47],recv_buff[i+48],recv_buff[i+49],recv_buff[i+50]);
                            // takeoff_flag_recv = recv_buff[i+51];
                            break;
                        }
                        else
                        {
                            recv_state = 0; 
                            s_info.fail_num++; 
                            i = i + Blen_host;
                        }
                    }
                    // 接收消息2
                    // else if (recv_buff[i+1] == 0xf1 && recv_buff[i+2] == 0x02 && (lend_recv-i) >= Blen_stop)
                    // {
                    //     cout << "2222222222222" << endl;
                    //     // 校验位检验
                    //     if (data_crc(&recv_buff[i],Blen_stop-2) == recv_buff[i+Blen_stop-2])
                    //     {
                    //         recv_state = 1;
                    //         s_info.recv_m2_r_num++;
                    //         // uav_ID = merge(recv_buff[i+3],recv_buff[i+4],recv_buff[i+5],recv_buff[i+6]);
                    //         if (recv_buff[i+3] & 0x1)
                    //             send_state = 1;
                    //         else 
                    //             send_state = 0;
                    //         // memset(recv_buff,0,sizeof(recv_buff));
                    //         // break;
                    //         i = i + Blen_stop;
                    //     }
                    //     else
                    //     {
                    //         recv_state = 0; 
                    //         s_info.fail_num++; 
                    //         i = i + Blen_stop;
                    //         // break;
                    //     }
                    // }
                    else
                        i++;
                }
                else
                    i++;
            }

}
uint8_t split(double data, int position)
{
    uint8_t result = 0;
    uint32_t data_32 = (uint32_t)(data*1e7);
    switch (position)
    {
        case 1:{result = (uint8_t)(data_32 >> 24);return result;}
        break;
        case 2:{result = (uint8_t)((data_32 & 0x00ff0000) >> 16);return result;}
        break;
        case 3:{result = (uint8_t)((data_32 & 0x0000ff00) >> 8);return result;}
        break;
        case 4:{result = (uint8_t)(data_32 & 0x000000ff);return result;}
        break;
    }
}

double merge(uint8_t first, uint8_t second, uint8_t third, uint8_t fourth)
{
    uint32_t temp = 0;
    double result = 0;
    temp = ((uint32_t)first<<24);
    temp |= ((uint32_t)second<<16);
	temp |= ((uint32_t)third<<8);
	temp |= ((uint32_t)fourth); 
    result = (double)(temp / 10000000.0);
}

uint8_t data_crc(uint8_t *p_data, int len_data)
{
    uint8_t result = 0;
    while (len_data--)
    {
        result = result ^ (*p_data++);
    }
    return result;
}